function dxdt = PN3_ode(t,x,pp)
    
    % Init_Data 质量 m,惯性半径r_Gx,r_Gy,r_Gz,气动参数 Cx0,Cy0,Cz0,参考面积 S_ref,重力加速度 g_0,空气密度 rho
    % 弹体气动力到质心距离 r_TBx,r_TBy,r_TBz,舵面气动力到质心距离 r_TDx,r_TDy,r_TDz
    m = PN3_Constant.m;
    r_Gx = PN3_Constant.r_Gx;r_Gy = PN3_Constant.r_Gy;r_Gz = PN3_Constant.r_Gz;
    %Cx0 = PN3_Constant.Cx0;Cy0 = PN3_Constant.Cy0;Cy_a = PN3_Constant.Cy_a;Cz0 = PN3_Constant.Cz0;Cz_b = PN3_Constant.Cz_b;
    S_ref = PN3_Constant.S_ref;L_ref = PN3_Constant.L_ref;g_0 = PN3_Constant.g_0;rho = PN3_Constant.rho;
    %mx = PN3_Constant.m_x;my = PN3_Constant.m_y;mz = PN3_Constant.m_z;
% 状态量 x = [x y z vx vy vz q0 q1 q2 q3 omega_x omega_y omega_z]
%g = [0;-g_0;0];
g = g_0;
rx = x(1);
ry = x(2);
rz = x(3);
vx = x(4);
vy = x(5);
vz = x(6);
v = sqrt(vx^2 + vy^2 + vz^2);
q_0 = x(7);
q_1 = x(8);
q_2 = x(9);
q_3 = x(10);
q = [q_0;q_1;q_2;q_3];
omega_x = x(11);
omega_y = x(12);
omega_z = x(13);


C_BI = [1-2*(q_2^2+q_3^2) , 2*(q_1*q_2+q_0*q_3) , 2*(q_1*q_3-q_0*q_2) ;
2*(q_1*q_2-q_0*q_3) , 1-2*(q_1^2+q_3^2) , 2*(q_2*q_3+q_0*q_1) ;
2*(q_1*q_3+q_0*q_2) , 2*(q_2*q_3-q_0*q_1) , 1-2*(q_1^2+q_2^2)];
vb = C_BI*[vx;vy;vz];
vbx = vb(1);vby = vb(2);vbz = vb(3);
%计算攻角和侧滑角 单位为度
%fprintf('x:%.2f,y:%.2f,z:%.2f\n',rx,ry,rz);
%fprintf('vx:%.2f,vy:%.2f,vz:%.2f\n',vx,vy,vz);
%fprintf('vbx:%.2f,vby:%.2f,vbz:%.2f\n',vbx,vby,vbz);
alpha = atan2d(-vby, vbx);  % 迎角 α (°)
beta  = asind(vbz / sqrt(vbx^2 + vby^2 + vbz^2)); % 侧滑角 β (°)
Ca = ppval(pp,max(min(alpha,10),-10));
mx = Ca(1);
my = Ca(2);
Cx = Ca(4);
Cy = Ca(5);
%两通道相同
Cb = ppval(pp,max(min(beta,10),-10));
mz = Cb(2);
Cz = Cb(5);
% mz = 0;
% Cz = 0;
fprintf('速度:%.4f,攻角:%.4f,侧滑角:%.4f\n',v, alpha,beta);
fprintf('Cx:%.4f,Cy:%.4f,Cz:%.4f,mx:%.4f,my:%.4f,mz:%.4f\n',Cx,Cy,Cz,mx,my,mz);
% 采用远程火箭常用的坐标系转换方式
%angle = my_quat2angle(q);
%fprintf('2-3-1欧拉角: phi:%.2f,psi:%.2f,gamma:%.2f\n', angle);
% dp Dynamic Pressure
dp = 0.5*rho*v^2;
%Cx = Cx0;Cy = Cy0 + Cy_a*max(min(alpha,15),-15);Cz = Cz0 + Cz_b*max(min(beta,15),-15);
Fxb = -Cx*dp*S_ref;
Fyb = Cy*dp*S_ref;
Fzb = -Cz*dp*S_ref;
Fxd = 0;Fyd = 0;Fzd = 0;    
M_x = mx*dp*S_ref*L_ref;
M_y = -mz*dp*S_ref*L_ref;
M_z = -my*dp*S_ref*L_ref;
fprintf('Fxb:%.4f,Fyb:%.4f,Fzb:%.4f,M_x:%.4f,M_y:%.4f,M_z:%.4f\n',Fxb,Fyb,Fzb,M_x,M_y,M_z);
% % 三维比例导引法：计算视线角速度和过载指令
% R_ref = [20.0-rx; 0.5-ry; -rz];  % 相对位置矢量
% V_ref = [-vx; -vy; -vz];  % 相对速度矢量
% R_ref_norm = sqrt((R_ref(1)^2 + R_ref(2)^2 + R_ref(3)^2));
% V_close = (R_ref(1)*V_ref(1) + R_ref(2)*V_ref(2) + R_ref(3)*V_ref(3)) / R_ref_norm;  % 相对速度在视线方向的投影

% % R_ref = [20.0-rx;0.5-ry;-rz];
% PN_alpha = atan2(-R_ref(3),R_ref(1));
% PN_beta = asin(R_ref(2)/(R_ref(1)^2+R_ref(2)^2+R_ref(3)^2)^0.5);
% % 视线矢量单位化
% %R_unit = R / norm(R);
% alpha_dot = (-V_ref(3)*R_ref(1) + V_ref(1)*R_ref(3)) / (R_ref(1)^2 + R_ref(3)^2);
% beta_dot = (R_ref_norm*V_ref(2) + V_close*R_ref(2)) / R_ref_norm^2; 
% %比例导引增益
% N = 3;
% ach = N * alpha_dot * V_close;
% acv = N * beta_dot * V_close;

% % 视线系投影到制导系
% ac1 = -ach*sin(PN_alpha)-acv*sin(PN_beta)*cos(PN_alpha);
% ac2 = ach*cos(PN_alpha)-acv*sin(PN_beta)*sin(PN_alpha);
% ac3 = acv*cos(PN_beta);


% a_cmd = [ac1;ac3;-ac2];
%a_cmd = C_BI.'*a_cmd;
% 制导系投影到仿真系
% 计算视线角速度
%omega_los = cross(R, V) / (norm(R)^2);
%fprintf('omega_los:%.4f,%.4f,%.4f\n',omega_los(1),omega_los(2),omega_los(3));
%fprintf();
% 比例导引增益


% 计算指令过载（比例导引律）
%a_cmd = N * cross(omega_los, V);
a_cmd = [0;0;0];
dxdt = [vx;
        vy;
        vz;
        -((Fxb + Fxd)*(2*q_2^2 + 2*q_3^2 - 1) + (2*q_0*q_3 - 2*q_1*q_2)*(Fyb + Fyd) - (2*q_0*q_2 + 2*q_1*q_3)*(Fzb + Fzd))/m + a_cmd(1);
        - g - ((Fyb + Fyd)*(2*q_1^2 + 2*q_3^2 - 1) - (2*q_0*q_3 + 2*q_1*q_2)*(Fxb + Fxd) + (2*q_0*q_1 - 2*q_2*q_3)*(Fzb + Fzd))/m + a_cmd(2);
        -((Fzb + Fzd)*(2*q_1^2 + 2*q_2^2 - 1) + (2*q_0*q_2 - 2*q_1*q_3)*(Fxb + Fxd) - (2*q_0*q_1 + 2*q_2*q_3)*(Fyb + Fyd))/m + a_cmd(3);
        - (omega_x*q_1)/2 - (omega_y*q_2)/2 - (omega_z*q_3)/2;
        (omega_x*q_0)/2 - (omega_y*q_3)/2 + (omega_z*q_2)/2;
        (omega_y*q_0)/2 + (omega_x*q_3)/2 - (omega_z*q_1)/2;
        (omega_y*q_1)/2 - (omega_x*q_2)/2 + (omega_z*q_0)/2;
        -(- m*omega_y*omega_z*r_Gy^2 + m*omega_y*omega_z*r_Gz^2 - M_x)/(m*r_Gx^2);
        (- m*omega_x*omega_z*r_Gx^2 + m*omega_x*omega_z*r_Gz^2 + M_y)/(m*r_Gy^2);
        -(- m*omega_x*omega_y*r_Gx^2 + m*omega_x*omega_y*r_Gy^2 - M_z)/(m*r_Gz^2)];
end